/*
 * Copyright (c) 2014, RoboPeak
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */
/*
 *  RoboPeak LIDAR System
 *  RPlidar ROS Node client test app
 *
 *  Copyright 2009 - 2014 RoboPeak Team
 *  http://www.robopeak.com
 *
 */

// 自己的函数
#include <iostream>
#include "headfile.h"
using namespace std;

constexpr double TWO_PI = 2 * M_PI;
constexpr double ONE_PI = M_PI;
// 初始化参数
mcq_msgs::Carry mcq_clustered_data;       // 自定义消息
int num_beams = Lidar_init.num_Scan;      // 定义接收多少束激光（358）
vector<int> isvalid(num_beams, 0);        // 初始化无效点
vector<int> DBSCAN_cluster(num_beams, 0); // 初始化DBSCAN的类别
vector<Feature_Parameters> F_P_before;
// detect参数
std::vector<float> detect_result;
void scanCallback(const sensor_msgs::LaserScan::ConstPtr &scan)
{

        // 设置随机数种子
        std::srand(std::time(0));
        auto start = std::chrono::high_resolution_clock::now();
        const int step = Lidar_init.ori_num_Scan / num_beams;
        int Gaosi_Nbefore;
        int Gaosi_Nafter;
        int Gaosi_Nbefore_two;
        int Gaosi_Nafter_two;
        for (size_t i = 0; i < num_beams; i++)
        {
                Lidar_Point p;
                p.x = double(i) / 10 * step;
                p.y = scan->ranges[i * step] * 100;
                Gaosi_Nafter = (i + 1 + num_beams) % num_beams;      // 考虑环形结构
                Gaosi_Nbefore = (i - 1 + num_beams) % num_beams;     // 考虑环形结构
                Gaosi_Nafter_two = (i + 2 + num_beams) % num_beams;  // 考虑环形结构
                Gaosi_Nbefore_two = (i - 2 + num_beams) % num_beams; // 考虑环形结构

                if ((scan->ranges[Gaosi_Nbefore * step] * 100 > Lidar_init.maxDistance && scan->ranges[Gaosi_Nafter * step] * 100 > Lidar_init.maxDistance) ||
                    (scan->ranges[i * step] * 100 > Lidar_init.maxDistance) ||
                    (fabs(scan->ranges[i * step] * 100 - scan->ranges[Gaosi_Nafter * step] * 100) >= 50 && fabs(scan->ranges[i * step] * 100 - scan->ranges[Gaosi_Nbefore * step] * 100) >= 50))
                {
                        p.x = 0;
                        p.y = Lidar_init.maxDistance;
                        // p.y = scan->ranges[i * step] * 100;
                        isvalid[i] = 1;
                }
                else if (scan->ranges[Gaosi_Nbefore * step] * 100 > Lidar_init.maxDistance && scan->ranges[Gaosi_Nafter * step] * 100 <= Lidar_init.maxDistance)
                {
                        p.y = 0.5 * scan->ranges[i * step] * 100 + 0.5 * scan->ranges[Gaosi_Nafter * step] * 100;
                        isvalid[i] = 0;
                }
                else if (scan->ranges[Gaosi_Nafter * step] * 100 > Lidar_init.maxDistance && scan->ranges[Gaosi_Nbefore * step] * 100 <= Lidar_init.maxDistance)
                {
                        p.y = 0.5 * scan->ranges[i * step] * 100 + 0.5 * scan->ranges[Gaosi_Nbefore * step] * 100;
                        isvalid[i] = 0;
                }
                else
                {
                        // p.y = scan->ranges[i * step] * 100;
                        p.y = 0.5 * scan->ranges[i * step] * 100 + 0.25 * scan->ranges[Gaosi_Nbefore * step] * 100 + 0.25 * scan->ranges[Gaosi_Nafter * step] * 100;
                        isvalid[i] = 0;
                }
                // 将坐标数据传给Lidar_data准备DBSCAN聚类
                // cout << "角度：" << p.x << "  距离：" << p.y << "  强度：" << scan->intensities[i * step] << endl;
                Myplus(Lidar_data, num_beams, i, p);
                // 类别参考和输出类别初始化
                mcq_clustered_data.cluster[i] = -1;
                DBSCAN_cluster[i] = -1;
        }
        // 将滤波后的点聚类
        DBSCAN(Lidar_data, Lidar_init.epsilon, Lidar_init.minPoints, DBSCAN_cluster, mcq_clustered_data.cluster, isvalid);
        map<int, vector<double>> Sort_angles;    // 字典形式存储每一类的角度，1：{a}         2：{a}
        map<int, vector<double>> Sort_distances; // 字典形式存储每一类的距离，1：{d}         2：{d}
        map<int, vector<double>> Sort_X_Axis;    // 字典形式存储每一类的X，1：{x}         2：{x}
        map<int, vector<double>> Sort_Y_Axis;    // 字典形式存储每一类的Y，1：{y}         2：{y}
        map<int, vector<int>> Sort_category;     // 字典形式存储每一类的激光束ID，1：{180}         2：{360}
        // 聚类完成后将数据发送给话题 // 新容器
        for (int i = 0; i < Lidar_data.size(); i++)
        {
                Lidar_Point p;
                p.x = Lidar_data[i].x;
                p.y = Lidar_data[i].y;
                p.category = mcq_clustered_data.cluster[i];
                Myplus(Lidar_class_data, num_beams, i, p);
                // 极坐标形式数据
                Sort_angles[mcq_clustered_data.cluster[i]].push_back(Lidar_class_data[i].x);    // 字典模式存储同一类别的角度数据(极坐标)
                Sort_distances[mcq_clustered_data.cluster[i]].push_back(Lidar_class_data[i].y); // 字典模式存储同一类别的距离数据(极坐标)
                double x_temp = 0;
                double y_temp = 0;
                // 直角坐标形式数据
                x_temp = Axis_x(Lidar_class_data[i].x, Lidar_class_data[i].y);
                y_temp = Axis_y(Lidar_class_data[i].x, Lidar_class_data[i].y);
                // cout << "角度：" << Lidar_data[i].x << "  距离：" << Lidar_data[i].y << endl;
                // cout << "x" << x_temp << "  y" << y_temp << endl;
                Sort_X_Axis[mcq_clustered_data.cluster[i]].push_back(x_temp); // 字典模式存储同一类别的x数据(直角坐标)
                Sort_Y_Axis[mcq_clustered_data.cluster[i]].push_back(y_temp); // 字典模式存储同一类别的y数据(直角坐标)
                // 每个点对应的类别
                Sort_category[mcq_clustered_data.cluster[i]].push_back(Lidar_class_data[i].category); // 字典模式存储同一类别的类型数据
                // 发给python端显示用
                if (mcq_clustered_data.cluster[i] == 0 || isvalid[i] == 1)
                {
#if is_Axis_display
                        // 直角坐标系
                        mcq_clustered_data.angles[i] = 0;
                        mcq_clustered_data.DB_data[i] = 0;
#else
                        mcq_clustered_data.angles[i] = Lidar_data[i].x;
                        mcq_clustered_data.DB_data[i] = Lidar_data[i].y;
#endif
                }
                else
                {
#if is_Axis_display
                        mcq_clustered_data.angles[i] = x_temp;
                        mcq_clustered_data.DB_data[i] = y_temp;
#else
                        mcq_clustered_data.angles[i] = Lidar_data[i].x;
                        mcq_clustered_data.DB_data[i] = 0;
#endif
                }
        }
        // 对每个聚类进行二乘拟合检测   Sort_category.size()表示有几类
        // 创建对应类数的特征提取
        const size_t Sort_category_size = Sort_category.size();
        vector<Feature_Parameters> F_P(Sort_category_size);
        F_P_before.resize(F_P.size());
        mcq_clustered_data.num_Points.resize(Sort_category_size, 0.0);
        mcq_clustered_data.average_angleDiff.resize(Sort_category_size, 0.0);
        mcq_clustered_data.centerX.resize(Sort_category_size, 0);
        mcq_clustered_data.centerY.resize(Sort_category_size, 0.0);
        mcq_clustered_data.radius.resize(Sort_category_size, 0.0);
        mcq_clustered_data.arc_length.resize(Sort_category_size, 0);
        mcq_clustered_data.average_widthDiff.resize(Sort_category_size, 0.0);
        mcq_clustered_data.average_curvature.resize(Sort_category_size, 0.0);
        mcq_clustered_data.sum_Point2Center.resize(Sort_category_size, 0);
        mcq_clustered_data.xPoint2Center_mean.resize(Sort_category_size, 0.0);
        mcq_clustered_data.yPoint2Center_mean.resize(Sort_category_size, 0.0);
        mcq_clustered_data.xPoint2SortX.resize(Sort_category_size, 0);
        mcq_clustered_data.yPoint2SortY.resize(Sort_category_size, 0.0);
        mcq_clustered_data.xPoint2averageX.resize(Sort_category_size, 0.0);
        mcq_clustered_data.yPoint2averageY.resize(Sort_category_size, 0);
        mcq_clustered_data.xKurtosis.resize(Sort_category_size, 0.0);
        mcq_clustered_data.yKurtosis.resize(Sort_category_size, 0.0);
        mcq_clustered_data.xSkewness.resize(Sort_category_size, 0);
        mcq_clustered_data.ySkewness.resize(Sort_category_size, 0.0);
        mcq_clustered_data.cen_x_zero_sum.resize(Sort_category_size, 0.0);
        mcq_clustered_data.zero_x_cen_sum.resize(Sort_category_size, 0.0);
        mcq_clustered_data.cen_y_zero_sum.resize(Sort_category_size, 0);
        mcq_clustered_data.zero_y_cen_sum.resize(Sort_category_size, 0.0);
        mcq_clustered_data.cen_x_sum.resize(Sort_category_size, 0.0);
        mcq_clustered_data.x_cen_sum.resize(Sort_category_size, 0.0);
        mcq_clustered_data.cen_y_sum.resize(Sort_category_size, 0);
        mcq_clustered_data.y_cen_sum.resize(Sort_category_size, 0.0);
        for (int Which_category = 1; Which_category < Sort_category_size; Which_category++)
        {
                /**************************************************常量定义区**************************************************/
                const size_t Sort_this_X_size = Sort_X_Axis[Which_category].size();
                /**************************************************拟合圆**************************************************/
                // 对该类别数据进行二乘拟合圆弧
                fitCircleToPoints(Sort_X_Axis[Which_category], Sort_Y_Axis[Which_category], F_P[Which_category].centerX, F_P[Which_category].centerY, F_P[Which_category].radius);
                F_P[Which_category].num_Points = Sort_this_X_size;
// 输出拟合出的圆心坐标和半径
#if is_printInfo
                cout
                    << "第" << Which_category << "类的点有：" << F_P[Which_category].num_Points << "个" << endl;
#endif
                /**************************************************其余特征参数**************************************************/
                /**** 保存相邻角度的夹角 ****/
                vector<double> angleDifferences; // 保存相邻角度的夹角
                /**** 计算线段宽度 ****/
                vector<double> segmentWidths; // 计算线段宽度
                /**** 保存各点的曲率 ****/
                vector<double> curvatures; // 保存各点的曲率
                /**** 保存各点到圆心的距离 ****/
                vector<double> Point2Center_distances; // 保存各点到圆心的距离
                /**** 坐标与圆心的距离 ****/
                vector<double> xPoint2Cemter; // X轴与中心的距离
                vector<double> yPoint2Cemter; // Y轴与中心的距离
                /**** 坐标与中位的距离 ****/
                vector<double> sortX = Sort_X_Axis[Which_category]; // X坐标到中位点的标准差
                vector<double> x2sortX;
                std::sort(sortX.begin(), sortX.end());
                double medianX = sortX[sortX.size() / 2];           // 中位值X
                vector<double> sortY = Sort_Y_Axis[Which_category]; // Y坐标到中位点的标准差
                vector<double> y2sortY;
                std::sort(sortY.begin(), sortY.end());
                double medianY = sortY[sortY.size() / 2]; // 中位值Y
                /**** 坐标与平均值的距离 ****/
                vector<double> x2average; // X坐标到平均坐标的方差
                double x_average = calculateAverage(Sort_X_Axis[Which_category]);
                vector<double> y2average; // Y坐标到平均坐标的方差
                double y_average = calculateAverage(Sort_Y_Axis[Which_category]);
                /**** 区域坐标的峰度 ****/
                vector<double> X_Kurtosis_sum1;
                vector<double> X_Kurtosis_sum2;
                vector<double> Y_Kurtosis_sum1;
                vector<double> Y_Kurtosis_sum2;
                /**** 区域的偏度 ****/
                vector<double> X_Skewness_sum;
                vector<double> Y_Skewness_sum;
                /**** 区域的坐标位置关系 ****/
                vector<double> cen_x_zero_sum;
                vector<double> zero_x_cen_sum;
                vector<double> cen_y_zero_sum;
                vector<double> zero_y_cen_sum;
                vector<double> x_cen_sum;
                vector<double> cen_x_sum;
                vector<double> y_cen_sum;
                vector<double> cen_y_sum;
                for (int j = 0; j < Sort_this_X_size; j++)
                {
                        if (j < Sort_this_X_size - 1) // 这个if只运行到倒数第二个点
                        {
                                // 计算相邻两个点的夹角
                                double angle1 = calculateAngle(F_P[Which_category].centerX, F_P[Which_category].centerY,
                                                               Sort_X_Axis[Which_category][j], Sort_Y_Axis[Which_category][j]);
                                double angle2 = calculateAngle(F_P[Which_category].centerX, F_P[Which_category].centerY,
                                                               Sort_X_Axis[Which_category][j + 1], Sort_Y_Axis[Which_category][j + 1]);
                                double angleDiff = angle2 - angle1;
                                // 将角度限制在[-π, π]范围内
                                if (angleDiff < -ONE_PI)
                                        angleDiff += TWO_PI;
                                else if (angleDiff > ONE_PI)
                                        angleDiff -= TWO_PI;
                                angleDifferences.push_back(angleDiff);

                                // 计算两个点的欧氏距离
                                double width = calculateDistance(Sort_X_Axis[Which_category][j], Sort_Y_Axis[Which_category][j],
                                                                 Sort_X_Axis[Which_category][j + 1], Sort_Y_Axis[Which_category][j + 1]);
                                segmentWidths.push_back(width);
                        }
                        // 计算该点的曲率
                        double distanceToCenter = calculateDistance(F_P[Which_category].centerX, F_P[Which_category].centerY,
                                                                    Sort_X_Axis[Which_category][j], Sort_Y_Axis[Which_category][j]);
                        double curvature = calculateCurvature(distanceToCenter);
                        curvatures.push_back(curvature);

                        // 计算该点到圆心的距离
                        Point2Center_distances.push_back(distanceToCenter);

                        // 计算坐标到圆心的差值
                        double xPoint2Cemter_temp = abs(Sort_X_Axis[Which_category][j] - F_P[Which_category].centerX);
                        double yPoint2Cemter_temp = abs(Sort_Y_Axis[Which_category][j] - F_P[Which_category].centerY);
                        xPoint2Cemter.push_back(xPoint2Cemter_temp);
                        yPoint2Cemter.push_back(yPoint2Cemter_temp);

                        // 计算坐标与中值的偏差
                        double xPoint2Sort_temp = pow(Sort_X_Axis[Which_category][j] - medianX, 2);
                        double yPoint2Sort_temp = pow(Sort_Y_Axis[Which_category][j] - medianY, 2);
                        x2sortX.push_back(xPoint2Sort_temp);
                        y2sortY.push_back(yPoint2Sort_temp);

                        // 计算坐标与平均值的偏差
                        double x2average_temp = pow(Sort_X_Axis[Which_category][j] - x_average, 2);
                        double y2average_temp = pow(Sort_Y_Axis[Which_category][j] - y_average, 2);
                        x2average.push_back(x2average_temp);
                        y2average.push_back(y2average_temp);

                        // 计算区域峰度
                        double X_Kurtosis_sum1_temp = pow(Sort_X_Axis[Which_category][j] - x_average, 4);
                        double X_Kurtosis_sum2_temp = pow(Sort_X_Axis[Which_category][j] - x_average, 2);
                        double Y_Kurtosis_sum1_temp = pow(Sort_Y_Axis[Which_category][j] - y_average, 4);
                        double Y_Kurtosis_sum2_temp = pow(Sort_Y_Axis[Which_category][j] - y_average, 2);
                        X_Kurtosis_sum1.push_back(X_Kurtosis_sum1_temp);
                        X_Kurtosis_sum2.push_back(X_Kurtosis_sum2_temp);
                        Y_Kurtosis_sum1.push_back(Y_Kurtosis_sum1_temp);
                        Y_Kurtosis_sum2.push_back(Y_Kurtosis_sum2_temp);

                        // 计算区域偏度
                        double X_skewness_diff = Sort_X_Axis[Which_category][j] - x_average;
                        double Y_skewness_diff = Sort_Y_Axis[Which_category][j] - y_average;
                        X_Skewness_sum.push_back(pow(X_skewness_diff, 3));
                        Y_Skewness_sum.push_back(pow(Y_skewness_diff, 3));

                        // 计算坐标区域关系
                        double zero_x_cen_temp = (Sort_X_Axis[Which_category][j] >= 0 && Sort_X_Axis[Which_category][j] <= F_P[Which_category].centerX) ? 1 : 0;
                        double cen_x_zero_temp = (Sort_X_Axis[Which_category][j] >= F_P[Which_category].centerX && Sort_X_Axis[Which_category][j] < 0) ? 1 : 0;
                        double zero_y_cen_temp = (Sort_Y_Axis[Which_category][j] >= 0 && Sort_Y_Axis[Which_category][j] <= F_P[Which_category].centerY) ? 1 : 0;
                        double cen_y_zero_temp = (Sort_Y_Axis[Which_category][j] >= F_P[Which_category].centerY && Sort_Y_Axis[Which_category][j] < 0) ? 1 : 0;
                        double x_cen_temp = (Sort_X_Axis[Which_category][j] < F_P[Which_category].centerX) ? 1 : 0;
                        double cen_x_temp = (Sort_X_Axis[Which_category][j] > F_P[Which_category].centerX) ? 1 : 0;
                        double y_cen_temp = (Sort_Y_Axis[Which_category][j] < F_P[Which_category].centerY) ? 1 : 0;
                        double cen_y_temp = (Sort_Y_Axis[Which_category][j] > F_P[Which_category].centerY) ? 1 : 0;
                        zero_x_cen_sum.push_back(zero_x_cen_temp);
                        cen_x_zero_sum.push_back(cen_x_zero_temp);
                        zero_y_cen_sum.push_back(zero_y_cen_temp);
                        cen_y_zero_sum.push_back(cen_y_zero_temp);
                        x_cen_sum.push_back(x_cen_temp);
                        cen_x_sum.push_back(cen_x_temp);
                        y_cen_sum.push_back(y_cen_temp);
                        cen_y_sum.push_back(cen_y_temp);
                }
                // 求取平均角度差
                F_P[Which_category].average_angleDiff = calculateAverage(angleDifferences) * 180 / ONE_PI;
#if is_printInfo
                cout << "第" << Which_category << "类的平均角度差为：" << F_P[Which_category].average_angleDiff << " 度" << endl;
#endif
// 求取圆弧半径
#if is_printInfo
                cout << "第" << Which_category << "类的圆弧半径：" << F_P[Which_category].radius << endl;
#endif
                // 计算圆弧长度
                double boundary_length = TWO_PI * F_P[Which_category].radius;                                          // 圆周长
                double arc_angle = fabs(accumulate(angleDifferences.begin(), angleDifferences.end(), 0.0) / (TWO_PI)); // 2pi中占多少
                F_P[Which_category].arc_length = arc_angle * boundary_length;
#if is_printInfo
                cout << "第" << Which_category << "类的弧长为：" << F_P[Which_category].arc_length << endl;
#endif
                // 计算平均线段宽度
                F_P[Which_category].average_widthDiff = calculateAverage(segmentWidths);
#if is_printInfo
                cout << "第" << Which_category << "类的平均线段宽度为：" << F_P[Which_category].average_widthDiff << endl;
#endif
                // 计算平均曲率
                F_P[Which_category].average_curvature = calculateAverage(curvatures);
#if is_printInfo
                cout << "第" << Which_category << "类的平均曲率为：" << F_P[Which_category].average_curvature << endl;
#endif
                // 计算区域内的点到圆心的距离和
                F_P[Which_category].sum_Point2Center = accumulate(Point2Center_distances.begin(), Point2Center_distances.end(), 0.0);
#if is_printInfo
                cout << "第" << Which_category << "类的距离和为：" << F_P[Which_category].sum_Point2Center << endl;
#endif
                // 计算区域内的点到圆心的坐标差均值
                F_P[Which_category].xPoint2Center_mean = calculateAverage(xPoint2Cemter);
                F_P[Which_category].yPoint2Center_mean = calculateAverage(yPoint2Cemter);
#if is_printInfo
                cout << "第" << Which_category << "类圆心的X均差：" << F_P[Which_category].xPoint2Center_mean << " Y均差：" << F_P[Which_category].yPoint2Center_mean << endl;
#endif
                // 计算区域内的点到中位坐标的标准差
                F_P[Which_category].xPoint2SortX = sqrt(calculateAverage(x2sortX));
                F_P[Which_category].yPoint2SortY = sqrt(calculateAverage(y2sortY));
#if is_printInfo
                cout << "第" << Which_category << "类中位的X标准差：" << F_P[Which_category].xPoint2SortX << " Y标准差：" << F_P[Which_category].yPoint2SortY << endl;
#endif
                // 计算区域内的点到平均值坐标的方差
                F_P[Which_category].xPoint2averageX = calculateAverage(x2average);
                F_P[Which_category].yPoint2averageY = calculateAverage(y2average);
#if is_printInfo
                cout << "第" << Which_category << "类平均的X标准差：" << F_P[Which_category].xPoint2averageX << " Y标准差：" << F_P[Which_category].yPoint2averageY << endl;
#endif
                // 计算区域峰度
                F_P[Which_category].xKurtosis = calculateKurtosis(X_Kurtosis_sum1, X_Kurtosis_sum2);
                F_P[Which_category].yKurtosis = calculateKurtosis(Y_Kurtosis_sum1, Y_Kurtosis_sum2);
#if is_printInfo
                cout << "第" << Which_category << "类X峰度：" << F_P[Which_category].xKurtosis << " Y峰度：" << F_P[Which_category].yKurtosis << endl;
#endif
                // 计算区域偏度
                F_P[Which_category].xSkewness = accumulate(X_Skewness_sum.begin(), X_Skewness_sum.end(), 0.0) / (Sort_this_X_size * pow(F_P[Which_category].xPoint2averageX, 1.5));
                F_P[Which_category].ySkewness = accumulate(Y_Skewness_sum.begin(), Y_Skewness_sum.end(), 0.0) / (Sort_this_X_size * pow(F_P[Which_category].yPoint2averageY, 1.5));
#if is_printInfo
                cout << "第" << Which_category << "类X偏度：" << F_P[Which_category].xSkewness << " Y偏度：" << F_P[Which_category].ySkewness << endl;
#endif
                // 计算区域坐标关系
                F_P[Which_category].cen_x_zero_sum = accumulate(cen_x_zero_sum.begin(), cen_x_zero_sum.end(), 0.0);
                F_P[Which_category].zero_x_cen_sum = accumulate(zero_x_cen_sum.begin(), zero_x_cen_sum.end(), 0.0);
                F_P[Which_category].cen_y_zero_sum = accumulate(cen_y_zero_sum.begin(), cen_y_zero_sum.end(), 0.0);
                F_P[Which_category].zero_y_cen_sum = accumulate(zero_y_cen_sum.begin(), zero_y_cen_sum.end(), 0.0);
                F_P[Which_category].cen_x_sum = accumulate(cen_x_sum.begin(), cen_x_sum.end(), 0.0);
                F_P[Which_category].x_cen_sum = accumulate(x_cen_sum.begin(), x_cen_sum.end(), 0.0);
                F_P[Which_category].cen_y_sum = accumulate(cen_y_sum.begin(), cen_y_sum.end(), 0.0);
                F_P[Which_category].y_cen_sum = accumulate(y_cen_sum.begin(), y_cen_sum.end(), 0.0);
#if is_printInfo
                cout << "第" << Which_category << "类X：" << F_P[Which_category].x_cen_sum << "       " << F_P[Which_category].cen_x_zero_sum << "       ";
                cout << F_P[Which_category].zero_x_cen_sum << "       " << F_P[Which_category].cen_x_sum << endl;
                cout << "第" << Which_category << "类Y：" << F_P[Which_category].y_cen_sum << "       " << F_P[Which_category].cen_y_zero_sum << "        ";
                cout << F_P[Which_category].zero_y_cen_sum << "       " << F_P[Which_category].cen_y_sum << endl;
#endif

                double score = 0;
                for (int i = 0; i < Lidar_init.Features_Nums; i++)
                {
                        score += (1 / (i + 1)) * (getFeature(F_P[Which_category], i) - getFeature(F_P_before[Which_category], i));
                }
                // cout << Which_category << "    的分：" << score / Lidar_init.Features_Nums << endl;
                // F_P_before[Which_category] = F_P[Which_category];
                Gaaosi_F_P(F_P[Which_category], Lidar_init.Features_Nums);
#if is_mcqMSG
                mcq_clustered_data.num_Points[Which_category] = F_P[Which_category].num_Points;
                mcq_clustered_data.average_angleDiff[Which_category] = F_P[Which_category].average_angleDiff;
                mcq_clustered_data.radius[Which_category] = F_P[Which_category].radius;
                mcq_clustered_data.arc_length[Which_category] = F_P[Which_category].arc_length;
                mcq_clustered_data.average_widthDiff[Which_category] = F_P[Which_category].average_widthDiff;
                mcq_clustered_data.average_curvature[Which_category] = F_P[Which_category].average_curvature;
                mcq_clustered_data.sum_Point2Center[Which_category] = F_P[Which_category].sum_Point2Center;
                mcq_clustered_data.xPoint2Center_mean[Which_category] = F_P[Which_category].xPoint2Center_mean;
                mcq_clustered_data.yPoint2Center_mean[Which_category] = F_P[Which_category].yPoint2Center_mean;
                mcq_clustered_data.xPoint2SortX[Which_category] = F_P[Which_category].xPoint2SortX;
                mcq_clustered_data.yPoint2SortY[Which_category] = F_P[Which_category].yPoint2SortY;
                mcq_clustered_data.xPoint2averageX[Which_category] = F_P[Which_category].xPoint2averageX;
                mcq_clustered_data.yPoint2averageY[Which_category] = F_P[Which_category].yPoint2averageY;
                mcq_clustered_data.xKurtosis[Which_category] = F_P[Which_category].xKurtosis;
                mcq_clustered_data.yKurtosis[Which_category] = F_P[Which_category].yKurtosis;
                mcq_clustered_data.xSkewness[Which_category] = F_P[Which_category].xSkewness;
                mcq_clustered_data.ySkewness[Which_category] = F_P[Which_category].ySkewness;
                mcq_clustered_data.cen_x_zero_sum[Which_category] = F_P[Which_category].cen_x_zero_sum;
                mcq_clustered_data.zero_x_cen_sum[Which_category] = F_P[Which_category].zero_x_cen_sum;
                mcq_clustered_data.cen_y_zero_sum[Which_category] = F_P[Which_category].cen_y_zero_sum;
                mcq_clustered_data.zero_y_cen_sum[Which_category] = F_P[Which_category].zero_y_cen_sum;
                mcq_clustered_data.x_cen_sum[Which_category] = F_P[Which_category].x_cen_sum;
                mcq_clustered_data.cen_x_sum[Which_category] = F_P[Which_category].cen_x_sum;
                mcq_clustered_data.y_cen_sum[Which_category] = F_P[Which_category].y_cen_sum;
                mcq_clustered_data.cen_y_sum[Which_category] = F_P[Which_category].cen_y_sum;
#endif
                if (Which_category == 1)
                        F_P[Which_category].is_Foot = 1;
                else
                        F_P[Which_category].is_Foot = 0;
        }

// 打开文件以写入特征数据，如果文件不存在则创建文件
#if is_OutputTxt
        std::ofstream outputFile("/home/mcq/ros1_lidar/src/rplidar_ros/src/features.txt", ios::app);
        outputFile << "-----------------------------Start-----------------------------" << endl;
        // 将每个区域的特征写入文件
        for (int i = 1; i < F_P.size(); i++)
        {
                const auto &feature = F_P[i];
                // 写入每个特征，使用逗号或其他适当的分隔符
                outputFile << i << ":"
                           << feature.num_Points << ","
                           << feature.average_angleDiff << ","
                           << feature.radius << ","
                           << feature.arc_length << ","
                           << feature.average_widthDiff << ","
                           << feature.average_curvature << ","
                           << feature.sum_Point2Center << ","
                           << feature.xPoint2Center_mean << "," << feature.yPoint2Center_mean << ","
                           << feature.xPoint2SortX << "," << feature.yPoint2SortY << ","
                           << feature.xPoint2averageX << "," << feature.yPoint2averageY << ","
                           << feature.xKurtosis << "," << feature.yKurtosis << ","
                           << feature.xSkewness << "," << feature.ySkewness << ","
                           << feature.cen_x_zero_sum << "," << feature.zero_x_cen_sum << ","
                           << feature.cen_y_zero_sum << "," << feature.zero_y_cen_sum << ","
                           << feature.x_cen_sum << "," << feature.cen_x_sum << ","
                           << feature.y_cen_sum << "," << feature.cen_y_sum << ","
                           << feature.is_Foot << endl;
        }
        outputFile << "-----------------------------End----------------------------" << endl;
        outputFile.close();
#endif
        // 计算程序执行的时间
        auto end = std::chrono::high_resolution_clock::now();
        std::chrono::duration<double> duration = end - start;
        std::cout << "程序1执行时间: " << duration.count() << " 秒" << std::endl;
}

void detectCallback(const mcq_msgs::py2cpp::ConstPtr &result)
{
        detect_result = result->isfoot;
        // 现在可以在 isfoot_data 中访问和处理接收到的 float32 数组数据
        for (size_t i = 0; i < detect_result.size(); ++i)
        {
                cout << detect_result[i];
        }
        cout << endl;
}

int main(int argc, char **argv)
{
        setlocale(LC_ALL, "zh_CN.UTF-8");
        ros::init(argc, argv, "rplidar_node_client");
        ros::NodeHandle n;
        mcq_clustered_data.angles.resize(num_beams, 0.0);
        mcq_clustered_data.DB_data.resize(num_beams, 0.0);
        mcq_clustered_data.cluster.resize(num_beams, 0);
        mcq_clustered_data.is_Foot.resize(num_beams, 0);
        ros::Publisher pub_DBdata = n.advertise<mcq_msgs::Carry>("/mcq_DB_topic", 1000);
        ros::Subscriber sub = n.subscribe<sensor_msgs::LaserScan>("/scan", 1000, &scanCallback);
        ros::Subscriber sub2 = n.subscribe<mcq_msgs::py2cpp>("/is_Foot_topic", 1000, &detectCallback);
        // typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::LaserScan, mcq_msgs::py2cpp> slamSyncPolicy;
        // message_filters::Subscriber<sensor_msgs::LaserScan> *lidar_sub; // topic1 输入
        // message_filters::Subscriber<mcq_msgs::py2cpp> *detect_sub;      // topic2 输入
        // message_filters::Synchronizer<slamSyncPolicy> *sync_;
        // lidar_sub = new message_filters::Subscriber<sensor_msgs::LaserScan>(n, "/scan", 1000);
        // detect_sub = new message_filters::Subscriber<mcq_msgs::py2cpp>(n, "/is_Foot_topic", 1000);
        // sync_ = new message_filters::Synchronizer<slamSyncPolicy>(slamSyncPolicy(10), *lidar_sub, *detect_sub);
        // sync_->registerCallback(boost::bind(&scanCallback, _1, _2));

        ros::Rate loop_rate(10);

        // 发布数据
        while (ros::ok())
        {
                pub_DBdata.publish(mcq_clustered_data);
                ros::spinOnce();

                loop_rate.sleep();
        }
        // ros::spin();
        return 0;
}